#include "qmath.h"

#include <math.h>

#include <stdio.h>

#define MULTIPLIER      7
#define PER_Q_DIV       166
#define SINTABLECOUNT   (4341)
#define SINTABLECOUNT4  (SINTABLECOUNT*4)

#include "sintable.h"


q32_t q32sin(q32_t v) {
    if (v < 0) v = -v + (SINTABLECOUNT4/2*MULTIPLIER*PER_Q_DIV);
    
    int index = (v * MULTIPLIER) / PER_Q_DIV;
	int index3 = (v * MULTIPLIER) % PER_Q_DIV;
    
    index = index % SINTABLECOUNT4; // overflow after 2pi
    
    q32_t aa;
    q32_t bb;
    
    //printf("%i %i\n",index,v);
    if (index < SINTABLECOUNT)
    {
        if (index >= SINTABLECOUNT - SIN_OVERFLOW) return 65536;
        
        aa = (q32_t)(sinTable[index]);
        bb = (q32_t)(sinTable[index+1]);
        
    }
    else if (index < SINTABLECOUNT*2)
    {
        if (index <= SINTABLECOUNT + SIN_OVERFLOW) return 65536;
        
        index = SINTABLECOUNT*2-index-1;
        aa = (q32_t)(sinTable[index+1]);
        bb = (q32_t)(sinTable[index]);
    }
    else if (index < SINTABLECOUNT*3)
    {
        if (index >= SINTABLECOUNT*3 - SIN_OVERFLOW) return -65536;
        index = index-SINTABLECOUNT*2;
        aa = (q32_t)(-sinTable[index]);
        bb = (q32_t)(-sinTable[index+1]);
    }
    else
    {
        if (index <= SINTABLECOUNT*3 + SIN_OVERFLOW) return -65536;
        index = SINTABLECOUNT*4-index-1;
        aa = (q32_t)(-sinTable[index+1]);
        bb = (q32_t)(-sinTable[index]);
    }
    aa = aa * PER_Q_DIV + (aa > 0 ? PER_Q_DIV / 2 : -PER_Q_DIV / 2);
    bb = bb * PER_Q_DIV + (bb > 0 ? PER_Q_DIV / 2 : -PER_Q_DIV / 2);
    
    q32_t cc = (aa * (PER_Q_DIV - index3) / PER_Q_DIV) + (bb * index3 / PER_Q_DIV);
    cc /= PER_Q_DIV;
    return cc;
}


q32_t q32cos(q32_t v) {
    return q32sin(v + (SINTABLECOUNT*MULTIPLIER*PER_Q_DIV));
}

int main()
{
	for (float f = -M_PI*4; f<M_PI*4; f+=0.00005f) //floatFromQ(PER_Q_DIV))
	{
        float a = floatFromQ(q32cos(qFromFloat(f)));
        float b = cosf(f);
		printf("%.16f;%.16f;%.16f;%.16f\n",f,b,a,(b-a)*65536.0f);
	}
    printf("\n%i\n",SINTABLECOUNT);
    
	return 0;
}
